#include "stdafx.h"
#include "PotentialField.h"

#include "Army.h"
#include "Unit.h"

#include "Globals.h"

using namespace AI;

XMVECTOR AI::getLinearPotentialFieldForce( XMVECTOR fieldPos, XMVECTOR queryPos, float scale )
{
	XMVECTOR queryToField = XMVectorSubtract(fieldPos, queryPos);
	return XMVectorScale( XMVector3Normalize(queryToField), scale );
}

XMVECTOR AI::getPotentialFieldForce( XMVECTOR fieldPos, XMVECTOR queryPos, float scale )
{
	XMVECTOR queryToField = XMVectorSubtract(fieldPos, queryPos);
	XMVECTOR dist = XMVector3Length(queryToField);
	XMVECTOR queryToFieldNorm = XMVectorDivide(queryToField, dist);

	float distSq = XMVectorGetX(dist) * XMVectorGetX(dist);

	return XMVectorScale( queryToFieldNorm, scale / distSq );
}

XMVECTOR AI::getAvoidArmyUnitsPFForce(Unit & queryingUnit)
{
	XMVECTOR queryPos =  XMVectorSetY(XMLoadFloat3(&queryingUnit.Position), 0);
	XMVECTOR pfForce = XMVectorZero();
	for(uint army = 0; army < g_gameScene->mNumArmies; army++)
	{
		const std::vector<Unit> & units = g_gameScene->mArmies[army]->mUnits;

		for(uint un = 0; un < units.size(); un++)
		{
			if(&units[un] == &queryingUnit)
				continue;
			XMVECTOR unitPos = XMVectorSetY(XMLoadFloat3(&units[un].Position), 0);
			pfForce = XMVectorAdd( pfForce, getPotentialFieldForce(unitPos, queryPos, -units[un].getBoundingSphere().Radius * 5000.0f) );
		}
	}
	
	return pfForce;
}
